///////////////////////////////////////////////////////////
//
//
//
///////////////////////////////////////////////////////////

// Local Includes
#include "stdafx.h"
#include "MyWindow.h"

// Haptic Control Includes
#include "HaptWiimote.h"

// SDL Includes
#include <sdl.h>
#include <SDL_opengl.h>

// System Includes

MyWindow::MyWindow()
: _pSurface(NULL),
  _font(),
  _w(1280),
  _h(1024),
  _buff(),
  _nMote(0),
  _bQuit(false)
{
}

MyWindow::~MyWindow()
{
}

bool
MyWindow::init()
{
    // Initialize SDL (Note: video is required to start event loop)
	if ( SDL_Init(SDL_INIT_VIDEO) < 0 ) {
		printf("ERROR - Initialization Failed: %s\n",SDL_GetError());
		return false;
	}

    // Initialize GL
    if (!_initGL())       return false;

    // Initialize the Joysticks
    if (!_initHaptics()) return false;
 
    if (!_font.init("Courier New")) return false;

    if (!HaptWiimote::connect()) return false;

    _nMote = min(4,HaptWiimote::numWiimotes());

    HaptWiimote *pMote = NULL;
    for ( int n=0; n<_nMote; n++ ) {
        pMote = HaptWiimote::wiimote(n);
        if (pMote) {
            pMote->enableThread(true);
            pMote->setMode(WII_OUTPUT_BUTMOTIR);
            pMote->setLEDs(1,0,0,0);
        }
    }
    
    return true;
}

bool
MyWindow::fini()
{
    if (!HaptWiimote::disconnect()) return false;

    SDL_Quit();
    return true;
}

bool 
MyWindow::_initGL()
{
    printf("***** Initializing OpenGL Window ***** \n");

    int bpp = SDL_GetVideoInfo()->vfmt->BitsPerPixel;
    unsigned int myFlags = SDL_OPENGL;
                      // | SDL_FULLSCREEN;

    SDL_GL_SetAttribute( SDL_GL_DOUBLEBUFFER, 1 );
    SDL_GL_SetAttribute( SDL_GL_ACCELERATED_VISUAL, 1 );
    SDL_GL_SetAttribute( SDL_GL_SWAP_CONTROL, 0 );

    _pSurface = SDL_SetVideoMode( _w, _h, bpp, myFlags );
    if ( _pSurface == NULL ) {
		printf("ERROR - SetVideoMode Failed: %s\n",SDL_GetError());
		return false;
	} 

    setViewPort(0,0,_w,_h);

    glBlendFunc(GL_SRC_ALPHA,GL_DST_ALPHA);
    glEnable(GL_BLEND);

    printf("***** Initializing OpenGL Complete ***** \n");
    
    printf("\n");
	printf( "Vendor     : %s\n", glGetString( GL_VENDOR ) );
	printf( "Renderer   : %s\n", glGetString( GL_RENDERER ) );
	printf( "Version    : %s\n", glGetString( GL_VERSION ) );
	printf( "Extensions : %s\n", glGetString( GL_EXTENSIONS ) );
	printf("\n");

    SDL_WM_SetCaption( "Wiicussion", "Fasten Your Wrist Straps" );


    return true;
}

bool 
MyWindow::_initHaptics()
{
    return true;
}

bool 
MyWindow::execute( int argv, char* argc[] )
{
    _bQuit = false;
    while  ( !_bQuit ) {
        if (!_preFrame())  break;
        if (!_render())    break;
        if (!_postFrame()) break;
    }

    return true;
}


bool
MyWindow::_preFrame()
{
    return _handleEvents();
}


bool
MyWindow::_postFrame()
{
    HaptWiimote *pMote = NULL;

    int mote;
    for ( mote=0; mote<_nMote; mote++ ) {
        pMote = HaptWiimote::wiimote(mote);
        if (pMote) {
            switch(mote) {
              case 0:
                  pMote->setLEDs(false,false,false,true);
                  break;
              case 1:
                  pMote->setLEDs(false,false,true,false);
                  break;
              default:
                pMote->setLEDs(false,false,false,false);
            };
        }
    }
    return true;
}


bool
MyWindow::_render()
{
	glClearColor( 0.0, 0.0, 0.0, 1.0 );
	glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glPointSize(10.0f);
    glColor4f( 0.0f, 1.0f, 0.0f, 1.0f );
    // Render the center point
    glBegin( GL_POINTS );
        glVertex2f( _w/2.0f, _h/2.0f );
    glEnd();

    _renderOrient();
    _renderXZ();
    _renderXY();
    _renderPointers();
    _renderText();

    SDL_GL_SwapBuffers( );

    return true;
}

bool 
MyWindow::_renderText()
{
    glColor4f( 0.0f, 0.0f, 1.0f, 1.0f );

    int off = 0;
    float x1, y1, x2, y2, x3, y3, x4, y4;
    unsigned int  x,  y,  z;
    float        fx, fy, fz;

    HaptWiimote *pMote = NULL;
    int mote;
    for ( mote=0; mote<_nMote; mote++ ) {
        pMote = HaptWiimote::wiimote(mote);
        
        sprintf( _buff, "Wiimote: %d", mote );
        _font.render( _buff, 10, _h-off-20 );

        pMote->getRawMotionData(x,y,z);
        pMote->getMotionData( fx, fy, fz );
        sprintf( _buff, "MotRaw( %d, %d, %d ) MotCal( %.2f, %.2f, %.2f) Pitch(%.2f) Roll(%.2f)", 
                 x, y, z, fx, fy, fz, pMote->pitch(), pMote->roll() );
        _font.render( _buff, 10, _h-off-40 );

        sprintf( _buff, "Button( a(%d) b(%d) 1(%d) 2(%d)", 
                pMote->isButtonPressed(WIIMOTE_BUTTON_A),
                pMote->isButtonPressed(WIIMOTE_BUTTON_B),
                pMote->isButtonPressed(WIIMOTE_BUTTON_1),
                pMote->isButtonPressed(WIIMOTE_BUTTON_2) );
        _font.render( _buff, 10, _h-off-60 );

        pMote->getIRData( x1, y1, x2, y2, x3, y3, x4, y4 );
        float test = x1*_w-_w/2.0f;
        sprintf( _buff, "IR1(%.2f,%.2f) IR2(%.2f,%.2f) IR3(%.2f,%.2f) IR4(%.2f,%.2f)", 
                 x1*_w, y1*_h, x2*_w, y2*_h, x3*_w, y3*_h, x4*_w, y4*_h );
        _font.render( _buff, 10, _h-off-80 );

        off += 100;
    }
    
    return true;
}

bool 
MyWindow::_renderPointers()
{
    float x1, y1, x2, y2, x3, y3, x4, y4;
    float cx, cy;

    static float aColor [][4] = { {1.0f,0.0f,0.0f,1.0f}, 
                                  {0.0f,1.0f,0.0f,1.0f} };

    HaptWiimote *pMote = NULL;
    int mote;
    for ( mote=0; mote<_nMote; mote++ ) {
        pMote = HaptWiimote::wiimote(mote);
        
        pMote->getRawIRData( x1, y1, x2, y2, x3, y3, x4, y4 );

        cx = (x1+x2+x3+x4)/4.0f; 
        cy = (y1+y2+y3+y4)/4.0f;

        glPointSize(5.0f);
        glColor4fv( aColor[mote] );

        // Render the 4 IR points
        glBegin( GL_POINTS );
            glVertex2f( x1+_w/2.0f, y1+_h/2.0f );
            glVertex2f( x2+_w/2.0f, y2+_h/2.0f );
            glVertex2f( x3+_w/2.0f, y3+_h/2.0f );
            glVertex2f( x4+_w/2.0f, y4+_h/2.0f );
        glEnd();

        pMote->getIRData( x1, y1, x2, y2, x3, y3, x4, y4 );
        
        /*
        x = cx+cos(roll)*(x1-cx)-sin(roll)*(y1-cy);
        y = cy+sin(roll)*(x1-cx)+cos(roll)*(y1-cy);
        x1 = x;
        y1 = y;

        x = cx+cos(roll)*(x2-cx)-sin(roll)*(y2-cy);
        y = cy+sin(roll)*(x2-cx)+cos(roll)*(y2-cy);
        x2 = x;
        y2 = y;

        x = cx+cos(roll)*(x3-cx)-sin(roll)*(y3-cy);
        y = cy+sin(roll)*(x3-cx)+cos(roll)*(y3-cy);
        x3 = x;
        y3 = y;

        x = cx+cos(roll)*(x4-cx)-sin(roll)*(y4-cy);
        y = cy+sin(roll)*(x4-cx)+cos(roll)*(y4-cy);
        x4 = x;
        y4 = y;
        */

        glColor4fv( aColor[mote+1] );
        glBegin( GL_POINTS );
            glVertex2f( x1+_w/2.0f, y1+_h/2.0f );
            glVertex2f( x2+_w/2.0f, y2+_h/2.0f );
            glVertex2f( x3+_w/2.0f, y3+_h/2.0f );
            glVertex2f( x4+_w/2.0f, y4+_h/2.0f );
        glEnd();

        glPointSize(10.0f);
        glColor4fv( aColor[0] );
        // Render the center point
        glBegin( GL_POINTS );
            glVertex2f( cx+_w/2.0f, cy+_h/2.0f );
        glEnd();
    }

    return true;
}

bool 
MyWindow::_renderXZ()
{
    // Ensure that OpenGL state is restored
    GLbitfield glAttribBits = GL_ENABLE_BIT   | 
                              GL_VIEWPORT_BIT | 
                              GL_DEPTH_BUFFER_BIT;
    glPushAttrib(glAttribBits);

    glViewport( _w-_w/3, 0, _w/3, _h/3 );

    glColor4f( 0.0f, 0.0f, 1.0f, 1.0f );

    glBegin(GL_LINE_LOOP);
        glVertex2f( 0.0f, 0.0f );
        glVertex2f( 0.0f, _h-1.0f );
        glVertex2f( _w-1.0f, _h-1.0f );
        glVertex2f( _w-1.0f, 0.0f );
    glEnd();

    int off = 80;
    float mx = _w/2.0f, my = _h-1.0f;
    float x=0.0f, y=0.0f, z=0.0f;

    HaptWiimote *pMote = NULL;
    int mote;
    for ( mote=0; mote<_nMote; mote++ ) {
        pMote = HaptWiimote::wiimote(mote);

        int p[4] = {0,1,2,3};
        pMote->_sort(p);

        // Render Labels for the IR points
        _font.render( "LL", pMote->_uiRawIR[p[0]][0]-512.0f+_w/2.0f, pMote->_uiRawIR[p[0]][1]-384.0f+_h/2.0f);
        _font.render( "UL", pMote->_uiRawIR[p[1]][0]-512.0f+_w/2.0f, pMote->_uiRawIR[p[1]][1]-384.0f+_h/2.0f);
        _font.render( "LR", pMote->_uiRawIR[p[2]][0]-512.0f+_w/2.0f, pMote->_uiRawIR[p[2]][1]-384.0f+_h/2.0f);
        _font.render( "UR", pMote->_uiRawIR[p[3]][0]-512.0f+_w/2.0f, pMote->_uiRawIR[p[3]][1]-384.0f+_h/2.0f);

        float lenLeft  = pMote->_length(pMote->_uiRawIR[p[0]],pMote->_uiRawIR[p[1]]);
        float lenRight = pMote->_length(pMote->_uiRawIR[p[2]],pMote->_uiRawIR[p[3]]);
        float lenUpper = pMote->_length(pMote->_uiRawIR[p[1]],pMote->_uiRawIR[p[3]]);
        float lenLower = pMote->_length(pMote->_uiRawIR[p[0]],pMote->_uiRawIR[p[1]]);

        
        sprintf(_buff," Left(%.3f) Right(%.3f) Top(%.3f) Bottom(%.3f) Avg(%.3f)",
                lenLeft, lenRight, lenUpper, lenLower, (lenLeft+lenRight+lenUpper+lenLower)/4.0f );
        _font.render(_buff,10,10+mote*off);

        float distLeft  = pMote->_distance(lenLeft);
        float distRight = pMote->_distance(lenRight);
        float distUpper = pMote->_distance(lenUpper);
        float distLower = pMote->_distance(lenLower);

        sprintf(_buff," distance( %.3f, %.3f, %.3f, %.3f )", 
                distLeft, distRight, distUpper, distLower );
        _font.render(_buff,10,30+mote*off);

        float C = acos( (100 + distLeft*distLeft - distRight*distRight)/(2*10*distLeft) );
        x = -distLeft*cos(C);
        z = distLeft*sin(C);

        float ap = pMote->pitch(),
              ar = pMote->roll();

        pMote->getPosition(x,y,z);

        sprintf( _buff," position( %.3f, %.3f, %.3f ) Pitch(%.3f) Roll(%.3f)", x, y, z, ap, ar ); 
        _font.render(_buff,10,50+mote*off);

        glBegin(GL_POINTS);
            glColor4f( 1.0f, 1.0f, 0.0f, 1.0f );
            glVertex2f( _w/2, _h-1.0f );
            glColor4f( 0.0f, 1.0f, 1.0f, 1.0f );
            glVertex2f( mx-2*x, my-2*z );
        glEnd();

    }

    glPopAttrib();

    return true;
}

bool 
MyWindow::_renderXY()
{
    // Ensure that OpenGL state is restored
    GLbitfield glAttribBits = GL_ENABLE_BIT   | 
                              GL_VIEWPORT_BIT | 
                              GL_DEPTH_BUFFER_BIT;
    glPushAttrib(glAttribBits);

    glViewport( _w-_w/3, _h/3, _w/3, _h/3 );

    glColor4f( 0.0f, 0.0f, 1.0f, 1.0f );

    glBegin(GL_LINE_LOOP);
        glVertex2f( 0.0f, 0.0f );
        glVertex2f( 0.0f, _h-1.0f );
        glVertex2f( _w-1.0f, _h-1.0f );
        glVertex2f( _w-1.0f, 0.0f );
    glEnd();

    HaptWiimote *pMote = NULL;
    int mote;
    for ( mote=0; mote<_nMote; mote++ ) {
        pMote = HaptWiimote::wiimote(mote);
    }

    glPopAttrib();

    return true;
}

bool
MyWindow::_renderOrient()
{    
    // Ensure that OpenGL state is restored
    GLbitfield glAttribBits = GL_ENABLE_BIT   | 
                              GL_VIEWPORT_BIT | 
                              GL_DEPTH_BUFFER_BIT;
    glPushAttrib(glAttribBits);

    glViewport( _w-_w/3, _h*2/3, _w/3, _h/3 );

    glColor4f( 0.0f, 0.0f, 1.0f, 1.0f );

    glBegin(GL_LINE_LOOP);
        glVertex2f( 0.0f,    0.0f );
        glVertex2f( 0.0f,    _h-1.0f );
        glVertex2f( _w-1.0f, _h-1.0f );
        glVertex2f( _w-1.0f, 0.0f );
    glEnd();

    HaptWiimote *pMote = NULL;
    int mote;
    for ( mote=0; mote<_nMote; mote++ ) {
        pMote = HaptWiimote::wiimote(mote);

        glPushMatrix();

        // NOTE: Reverse order
        glTranslatef(_w/2.0f,_h/2.0f,0.0f);
        glRotatef(pMote->pitch(),1.0f,0.0f,0.0f);
        glRotatef(pMote->roll(),0.0f,0.0f,1.0f);
        
        glColor4f( 1.0f, 0.0f, 0.0f, 1.0f );

        glBegin(GL_TRIANGLE_STRIP);
            glVertex3f( 35.0f, -30.0, 148.75f );
            glVertex3f( 35.0f, 30.0f, 148.75f );
            glVertex3f( -35.0f, -30.0f, 148.75f );
            glVertex3f( -35.0f, 30.0f, 148.75f );
        glEnd();
        
        glColor4f( 0.0f, 0.0f, 1.0f, 1.0f );
        glBegin(GL_TRIANGLE_STRIP);
            glVertex3f( 35.0f, -30.0, -148.75f );
            glVertex3f( 35.0f, 30.0f, -148.75f );
            glVertex3f( 35.0f, -30.0, 148.75f );
            glVertex3f( 35.0f, 30.0f, 148.75f );
        glEnd();
        glBegin(GL_TRIANGLE_STRIP);
            glVertex3f( -35.0f, 30.0, -148.75f );
            glVertex3f( 35.0f, 30.0f, -148.75f );
            glVertex3f( -35.0f, 30.0, 148.75f );
            glVertex3f( 35.0f, 30.0f, 148.75f );
        glEnd();
        glBegin(GL_TRIANGLE_STRIP);
            glVertex3f( -35.0f, -30.0, -148.75f );
            glVertex3f( -35.0f, 30.0f, -148.75f );
            glVertex3f( -35.0f, -30.0, 148.75f );
            glVertex3f( -35.0f, 30.0f, 148.75f );
        glEnd();
        glBegin(GL_TRIANGLE_STRIP);
            glVertex3f( -35.0f, -30.0, -148.75f );
            glVertex3f( 35.0f, -30.0f, -148.75f );
            glVertex3f( -35.0f, -30.0, 148.75f );
            glVertex3f( 35.0f, -30.0f, 148.75f );
        glEnd();
        
        glColor4f( 0.0f, 1.0f, 0.0f, 1.0f );

        glBegin(GL_TRIANGLE_STRIP);
            glVertex3f( -35.0f, 30.0f, -148.75f );
            glNormal3f( 0.0f, 0.0f, -1.0f );
            glVertex3f( -35.0f, -30.0f, -148.75f );
            glNormal3f( 0.0f, 0.0f, -1.0f );
            glVertex3f( 35.0f, 30.0f, -148.75f );
            glNormal3f( 0.0f, 0.0f, -1.0f );
            glVertex3f( 35.0f, -30.0f, -148.75f );
            glNormal3f( 0.0f, 0.0f, -1.0f );
        glEnd();
        glPopMatrix();
    }

    glPopAttrib();

    return true;
}

bool
MyWindow::_handleEvents()
{
    SDL_Joystick* pJoy = NULL; 
	bool bSustain=false;

    SDL_Event   event;
    while ( SDL_PollEvent(&event) ) {
	    switch (event.type) {
            case SDL_KEYDOWN: 
                if (event.key.keysym.sym == SDLK_g) {
                    break;
                }
                if ( event.key.keysym.sym != SDLK_ESCAPE ) {
                    break;  // Allow escape key to be used to quit
                }
            case SDL_QUIT:
			    _bQuit = 1;
			break;
		    default:
			break;
        }
    }

    return true;
}

bool 
MyWindow::setViewPort( int x, int y, int w, int h )
{
    glViewport( x, y, w, h );
    glMatrixMode( GL_PROJECTION );
	glLoadIdentity( );

    //glOrtho( x, w, y, h, -20.0, 20.0 );
	glOrtho( x, w, y, h, -1000.0, 1000.0 );

	glMatrixMode( GL_MODELVIEW );
	glLoadIdentity( );

    return true;
}

//////////////////////////////
// 
//////////
